/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * command.cxx
 *
 * PURPOSE:
 *  This code implements the command data structure between tasks on 
 *  the flight computer and from ground control station
 *
 * CREATED:
 * 11/2000 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/09/09 19:35:44 $
 * $Revision: 1.8 $
 *
 ***********************************************************************/

#include <stdio.h>
#include <string.h>
#include <assert.h>

#include "zigbee.h"

// these are used for COUT
using std::hex;
using std::dec;
 

//////////////////////////////////////////////////////////////////////
// The ZigbeeQueue Class
//////////////////////////////////////////////////////////////////////

// private functions
uint8_t ZigbeeQueue::APIcalcCheckSum() const 
{
  	register uint32_t sum = 0;
	uint8_t Csum= 0, val= 0;
	int length = API_SIZE(zigbee_fifo);

	// sum up all bytes
	for(int i= API_DELIMETER_SIZE + API_LENGTH_SIZE; i < length - API_CSUM_SIZE; i++){
		sum += zigbee_fifo[i]; 
	}

	//take LSB
	Csum= 0xFF - (uint8_t)(sum & 0x000000FF);
	
	val= zigbee_fifo[ API_NUM(zigbee_fifo) + API_DELIMETER_SIZE +  API_LENGTH_SIZE  ];

	return (Csum - val);
}

void ZigbeeQueue::APIsetCheckSum(uint8_t message[], uint8_t size)
{

  API_CSUM(message)= 0;
  API_CSUM(message)= APIcalcTXCheckSum(message, size);
}

uint8_t ZigbeeQueue::APIcalcTXCheckSum(uint8_t message[], uint8_t size) const
{
  uint32_t sum = 0;
 	uint8_t Csum= 0;
	size= size + API_TX_HEADER_SIZE + API_CSUM_SIZE;

 	for(int i= API_DELIMETER_SIZE +  API_LENGTH_SIZE; i < size - API_CSUM_SIZE; i++){
		sum += message[i];
	}
	//take LSB
	Csum= 0xFF - (uint8_t)(sum & 0x000000FF);

	return Csum;
}

// append to the queue, a buffer of length num
void ZigbeeQueue::push(uint8_t * buf, size_t num) 
{
  for(register size_t i=0; i < num ; i++){
    zigbee_fifo.push_back( buf[i] );
  }
}

// pop a single byte off of the queue
uint8_t ZigbeeQueue::popByte() 
{
	assert( ! zigbee_fifo.empty() );
	uint8_t val = zigbee_fifo.front();
	zigbee_fifo.erase(zigbee_fifo.begin());

	return val;
}

// checks to see if the next set of bytes on the byte
// queue form a valid command
bool ZigbeeQueue::APIisValid() const {
  if(  zigbee_fifo.size() < (size_t)API_SIZE(zigbee_fifo) ){
		return false;
  }
  else{
	return ( APIcalcCheckSum() == 0 ? true : false);
  }
}


bool ZigbeeQueue::APIneedMoreBytes() const
{
  //cout << "zigbee_fifo size: " << zigbee_fifo.size() << "   ";
  //cout << "API_SIZE: "; 
  //printf("%d",(size_t)API_SIZE(zigbee_fifo));
  //cout << endl;

  if( zigbee_fifo.size() < (size_t)API_SIZE(zigbee_fifo) ){
    return true;
  }
  else{
    return false;
  }
}

uint16_t ZigbeeQueue::getMavNum() 
{
  uint16_t Num, val[2];
  val[0]= zigbee_fifo[4];//MSB
  val[1]= zigbee_fifo[5];//LSB

  Num= (val[0]<<8) + val[1];
  return Num;
}

void ZigbeeQueue::requestAddress(uint8_t * buf) 
{
  buf[0] = 0x7E;
  buf[1] = 0x00;
  buf[2] = 0x04;
  buf[3] = 0x09;
  buf[4] = 0x01;
  buf[5] = 'M';
  buf[6] = 'Y';
  buf[7] = 0;
	APIsetCheckSum(buf,0);
}

void ZigbeeQueue::popCommand()
{

  assert( ! zigbee_fifo.empty() );
 uint8_t size= API_SIZE(zigbee_fifo);
  zigbee_fifo.erase( zigbee_fifo.begin(), zigbee_fifo.begin() + size);
}

uint16_t ZigbeeQueue::size()
{
  return API_SIZE(zigbee_fifo);
}

uint8_t ZigbeeQueue::getByte(uint8_t j)
{
  assert( ! zigbee_fifo.empty() );
  uint8_t val=0;
  val= zigbee_fifo.at(j);

  return val;
}

void ZigbeeQueue::APIheaders(uint8_t array[], uint16_t destination, uint8_t size)
{
  array[0]= 0x7E;
  array[1]= 0;
  array[2]= size + API_ID_SIZE + API_FRAME_SIZE + API_ADDRESS_SIZE + API_OPTIONS_SIZE;
  array[3]= 0x01;
  array[4]=0;
  array[5]= (uint8_t)((destination>>8) & 0x00FF);
  array[6]= (uint8_t)(destination & 0x00FF);
  array[7]= 0x01;
}
